/* INCLUSIONS */
#include "WFOLLOW.h"

/* PRIVATE VARIABLES */
double IR_DIST = 8.6;

/* FUNCTION BODIES */

void WFOLLOW_INIT(void) {
  SERIAL_INIT();
  init_proximity_sensors();
}

void WFOLLOW(void) {
  while(1) { 
    if (read_sensor_distance(RF_SENSOR) < read_sensor_distance(RR_SENSOR)) {
      ROTATE_CCLKW((uint32_t) 180 - ceil(acos((read_sensor_distance(RR_SENSOR)-read_sensor_distance(RF_SENSOR)) / IR_DIST)));
    }
    if (read_sensor_distance(RF_SENSOR) > read_sensor_distance(RR_SENSOR)) {
      ROTATE_CLKW((uint32_t) ceil(acos((read_sensor_distance(RF_SENSOR) - read_sensor_distance(RR_SENSOR))/IR_DIST)));
    }
  }
}

